#ifndef FIXED_H_
#define FIXED_H_

#include <boost/integer.hpp>
#include <string>
#include <sstream>

namespace uav
{
namespace math
{

const int FIXED_INT_BITS = 36; ///< Number of bits in the integral part of a fixed-point value.
const int FIXED_FRAC_BITS = 28; ///< Number of bits in the fractional part of a fixed-point value.
const int64_t FIXED_RESOLUTION = (int64_t(1) << FIXED_FRAC_BITS); ///< The %internal fixed-point representation for the value one.
const int64_t FIXED_INT_MASK = 0xfffffffff0000000LL; ///< A bit-mask for the integral part of a fixed-point value.
const int64_t FIXED_FRAC_MASK = 0x000000000fffffffLL; ///< A bit-mask for the fractional part of a fixed-point value.

/**
 * @class Fixed
 *
 * @brief Fixed-point arithmetic implementation using a 36.28 format.
 *
 * This implementation is based on the work of Anthony Williams.
 * All trigonometric funtions assume angles are in radians.
 *
 * @author Travis Portz
 */
class Fixed
{
private:
	int64_t val;

	/**
	 * @brief Used for constructing Fixed objects directly from their %internal representations.
	 */
	struct internal {};

	/**
	 * Constructs a Fixed object with the %internal bit representation given by @a v.
	 *
	 * @param i The empty internal struct @c internal().
	 * @param v The %internal bit representation of the value to be constructed.
	 */
	Fixed(internal i, int64_t v) : val(v) {}

public:

	/**
	 * Constructs a Fixed object with value zero.
	 */
	Fixed() : val(0) {}

	/**
	 * Constructs a Fixed object with the integer value @a i.
	 */
	explicit Fixed(int i) : val(int64_t(i) << FIXED_FRAC_BITS) {}

	/**
	 * Constructs a Fixed object with the integer value @a v.
	 */
	explicit Fixed(int64_t v) : val(v << FIXED_FRAC_BITS) {}

	/**
	 * Constructs a Fixed object from the double @a d.
	 */
	explicit Fixed(double d);

	/**
	 * Constructs a Fixed object from the float @a f.
	 */
	explicit Fixed(float f);

	/**
	 * Constructs a Fixed object from the string @a str.
	 */
	explicit Fixed(const std::string &str);

	/**
	 * Get the internal bit representation of the fixed-point number.
	 */
	const int64_t& bits() const { return val; }

	/**
	 * Converts the fixed-point value to a double.
	 */
	double toDouble() const;

	/**
	 * Converts the fixed-point value to a float.
	 */
	float toFloat() const;

	/**
	 * Converts the fixed-point value to an integer by truncating towards zero.
	 *
	 * This method should not be used for values greater than @f$ 2^{32} @f$.
	 */
	int toInt() const { return int(val >> FIXED_FRAC_BITS); }

	/**
	 * Converts the fixed-point value to a string using the default floating-point notation.
	 */
	std::string toString() const;

	/**
	 * Simultaneously computes the sine and cosine of this value using the CORDIC algorithm.
	 *
	 * @param[out] s The sine of this value.
	 * @param[out] c The cosine of this value.
	 */
	void sinCos(Fixed &s, Fixed &c) const;

	/**
	 * Computes four quadrant arctangent of @a y / @a x.
	 *
	 * @note Use the toPolar() function if you also need to compute the value @f$ \sqrt{y / x} @f$.
	 */
	static Fixed atan2(const Fixed &y, const Fixed &x);

	/**
	 * Converts rectangular coordinates (@a x, @a y) to polar coordinates (@a r, @a theta) using the CORDIC algorithm.
	 */
	static void toPolar(const Fixed &x, const Fixed &y, Fixed &r, Fixed &theta);

	/**
	 * Rounds the fixed-point value towards negative infinity.
	 */
	Fixed floor() const
	{
		return Fixed(internal(), val & FIXED_INT_MASK);
	}

	/**
	 * Rounds the fixed-point value towards positive infinity.
	 */
	Fixed ceil() const
	{
		return Fixed(internal(), (val + FIXED_FRAC_MASK) & FIXED_INT_MASK);
	}

	/**
	 * Returns the absolute value of this fixed-point value.
	 */
	Fixed abs() const
	{
		return (val < 0) ? Fixed(internal(), -val) : *this;
	}

	/**
	 * Computes the square-root of this fixed-point value.
	 */
	Fixed sqrt() const;

	/**
	 * Computes the exponential of this fixed-point value.
	 */
	Fixed exp() const;

	/**
	 * Computes the natural logarithm of this fixed-point value.
	 */
	Fixed log() const;

	/**
	 * Computes the sine of this value.
	 *
	 * @note Use the sinCos() function if you will also be needing the cosine of this value.
	 */
	Fixed sin() const
	{
		Fixed s, c;
		sinCos(s, c);
		return s;
	}

	/**
	 * Computes the cosine of this value.
	 *
	 * @note Use the sinCos() function if you will also be needing the sine of this value.
	 */
	Fixed cos() const
	{
		Fixed s, c;
		sinCos(s, c);
		return c;
	}

	/**
	 * Computes the tangent of this value.
	 *
	 * @note Obtain the tangent from the sinCos() function if you will also be needing the sine and/or cosine of this value.
	 */
	Fixed tan() const
	{
		Fixed s, c;
		sinCos(s, c);
		return s / c;
	}

	/**
	 * Computes the arctangent of this value.
	 *
	 * @note Use the atan2() or toPolar() function if you are computing @f$ \arctan (y/x) @f$
	 * and do not need the value @f$ y/x @f$ for any other purposes.
	 */
	Fixed atan() const
	{
		return atan2(*this, One);
	}

	/**
	 * Returns the maximum of @a x and @a y.
	 */
	static Fixed max(const Fixed &x, const Fixed &y)
	{
		return (x.val > y.val) ? x : y;
	}
	/**
	 * Returns the minimum of @a x and @a y.
	 */
	static Fixed min(const Fixed &x, const Fixed &y)
	{
		return (x.val < y.val) ? x : y;
	}

	static const Fixed Inf; ///< The maximum Fixed value, used to represent infinity.
	static const Fixed Ninf; ///< The minimum Fixed value, used to represent negative infinity.
	static const Fixed Two; ///< The value two.
	static const Fixed One; ///< The value one.
	static const Fixed Half; ///< The value @f$ \frac{1}{2} @f$.
	static const Fixed Quarter; ///< The value @f$ \frac{1}{4} @f$.
	static const Fixed Zero; ///< The value zero.
	static const Fixed Pi; ///< The value @f$ \pi @f$.
	static const Fixed TwoPi; ///< The value @f$ 2 \pi @f$.
	static const Fixed HalfPi; ///< The value @f$ \frac{\pi}{2} @f$.
	static const Fixed QuarterPi; ///< The value @f$ \frac{\pi}{4} @f$.

	Fixed& operator+=(const Fixed &other)
	{
		val += other.val;
		return *this;
	}
	Fixed& operator-=(const Fixed &other)
	{
		val -= other.val;
		return *this;
	}
	Fixed& operator*=(const Fixed &other);
	Fixed& operator/=(const Fixed &other);

	const Fixed operator+(const Fixed &other) const
	{
		return Fixed(internal(), val + other.val);
	}
	const Fixed operator-(const Fixed &other) const
	{
		return Fixed(internal(), val - other.val);
	}
	const Fixed operator*(const Fixed &other) const
	{
		Fixed res = *this;
		res *= other;
		return res;
	}
	const Fixed operator/(const Fixed &other) const
	{
		Fixed res = *this;
		res /= other;
		return res;
	}

	Fixed& operator++()
	{
		val += FIXED_RESOLUTION;
		return *this;
	}
	Fixed operator++(int)
	{
		Fixed temp = *this;
		val += FIXED_RESOLUTION;
		return temp;
	}
	Fixed& operator--()
	{
		val -= FIXED_RESOLUTION;
		return *this;
	}
	Fixed operator--(int)
	{
		Fixed temp = *this;
		val -= FIXED_RESOLUTION;
		return temp;
	}

	Fixed operator-() const { return Fixed(internal(), -val); }

	bool operator==(const Fixed &other) const { return val == other.val; }
	bool operator!=(const Fixed &other) const { return val != other.val; }
	bool operator<(const Fixed &other) const { return val < other.val; }
	bool operator>(const Fixed &other) const { return val > other.val; }
	bool operator<=(const Fixed &other) const { return val <= other.val; }
	bool operator>=(const Fixed &other) const { return val >= other.val; }

	Fixed& operator<<=(unsigned int i)
	{
		val <<= i;
		return *this;
	}
	Fixed& operator>>=(unsigned int i)
	{
		val >>= i;
		return *this;
	}

	Fixed operator<<(unsigned int i) const
	{
		return Fixed(internal(), val << i);
	}
	Fixed operator>>(unsigned int i) const
	{
		return Fixed(internal(), val >> i);
	}
};

/**
 * Writes a fixed-point value to an output stream as a double.
 */
inline std::ostream& operator<<(std::ostream &os, const Fixed &val)
{
	return os << val.toDouble();
}

/**
 * Reads a double from an input stream and converts it to a fixed-point value.
 */
inline std::istream& operator>>(std::istream &is, Fixed &val)
{
	double d;
	is >> d;
	val = Fixed(d);
	return is;
}

inline Fixed::Fixed(const std::string &str)
{
	std::istringstream iss(str);
	iss >> *this;
}

inline std::string Fixed::toString() const
{
	std::ostringstream oss;
	oss.precision(10);
	oss << toDouble();
	return oss.str();
}


/**
 * A convenience function to round a fixed-point number towards negative infinity.
 *
 * @see Fixed::floor
 */
inline Fixed floor(const Fixed &val) { return val.floor(); }

/**
 * A convenience function to round a fixed-point number towards positive infinity.
 *
 * @see Fixed::ceil
 */
inline Fixed ceil(const Fixed &val) { return val.ceil(); }

/**
 * A convenience function to obtain the absolute value of a fixed-point number.
 *
 * @see Fixed::abs
 */
inline Fixed abs(const Fixed &val) { return val.abs(); }

/**
 * A convenience function to compute the square root of a fixed-point number.
 *
 * @see Fixed::sqrt
 */
inline Fixed sqrt(const Fixed &val) { return val.sqrt(); }

/**
 * A convenience function to compute the exponential of a fixed-point number.
 *
 * @see Fixed::exp
 */
inline Fixed exp(const Fixed &val) { return val.exp(); }

/**
 * A convenience function to compute the natural logarithm of a fixed-point number.
 *
 * @see Fixed::log
 */
inline Fixed log(const Fixed &val) { return val.log(); }

/**
 * A convenience function to compute the sine of a fixed-point value.
 *
 * @note Use the Fixed::sinCos function if you will also be needing the cosine of the value.
 */
inline Fixed sin(const Fixed &val) { return val.sin(); }

/**
 * A convenience function to compute the cosine of a fixed-point value.
 *
 * @note Use the Fixed::sinCos function if you will also be needing the sine of the value.
 */
inline Fixed cos(const Fixed &val) { return val.cos(); }

/**
 * A convenience function to compute the tangent of a fixed-point value.
 *
 * @note Obtain the tangent using the Fixed::sinCos function if you will also be needing the sine and/or cosine of the value.
 */
inline Fixed tan(const Fixed &val) { return val.tan(); }

/**
 * Computes the arctangent of a fixed-point value.
 *
 * @note Use the Fixed::toPolar function if you are computing atan(y/x) and do not need the value y/x for any other purposes.
 */
inline Fixed atan(const Fixed &val) { return val.atan(); }

}} // end namespace

#endif
